package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.*;
//import java.util.Timer;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class SkyrimAutoTest extends IterativeRobot {
    Joystick joyRight = new Joystick (1);                                      // Right Joystick is in Port 1
    Joystick joyLeft = new Joystick (2);
    Gyro Orange = new Gyro(2);
    DigitalInput leftline = new DigitalInput (1);
    DigitalInput centerline = new DigitalInput (3);
    DigitalInput rightline = new DigitalInput (5);
    RobotDrive OrangeDrive = new RobotDrive (1,3,2,4);
    Timer timelapse = new Timer();
    double currenttime = 0;
    
    public void robotInit() {
       System.out.println("ROBOT INIT IS GETTING CALLED!");
       Orange.reset();
       OrangeDrive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
       OrangeDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
       OrangeDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, false);
        
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousContinuous() {        
         timelapse.start();
         while(currenttime <= 2.50){
             OrangeDrive.mecanumDrive_Cartesian(0,.2,0,0);
             currenttime = timelapse.get();  
            }
            OrangeDrive.mecanumDrive_Cartesian(0,0,0,0);
          //-----  timelapse.reset();
         }
    public void disabledContinuous() {
        
        timelapse.reset();
    }

    

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
      
    
    }
}
